
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "esp_mac.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "lwip/err.h"
#include "lwip/sys.h"
#include "esp_log.h"
#include "esp_system.h"
#include "nvs_flash.h"
#include "sys/param.h"
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/gpio.h"
#include "spi_flash_mmap.h"
#include "esp_http_server.h"
#include "esp_spiffs.h"
#include "driver/ledc.h"
#include "esp_camera.h"
#include "esp_timer.h"

#include "color_detect.h"
#include "motion.h"

static const char *TAG = "main";
static int led_brightness = 25;  // 默认亮度25%
static httpd_handle_t camera_httpd = NULL;  // 主服务器
static httpd_handle_t stream_httpd = NULL;  // 流服务器
static Point color_detect_points[6]= {{86, 59}, {169, 27}, {280, 62}, {78, 185}, {158, 218}, {272, 203}};
static Point color_detect_quads[18][4] = {0};
static SemaphoreHandle_t camera_mutex = NULL; // 摄像头操作互斥锁
// 添加全局变量用于存储图像缓冲区
static uint8_t *s_fb_buf = NULL;
static size_t s_fb_buf_len = 0;
static SemaphoreHandle_t s_fb_buf_mutex = NULL; // 保护图像缓冲区的互斥锁

#define CAM_PIN_D2    3
#define CAM_PIN_D1    4
#define CAM_PIN_D3    5
#define CAM_PIN_D0    6
#define CAM_PIN_D4    7
#define CAM_PIN_PCLK  8
#define CAM_PIN_D5    9
#define CAM_PIN_D6    10
#define CAM_PIN_XCLK  11
#define CAM_PIN_D7    12
#define CAM_PIN_HREF  13
#define CAM_PIN_PWDN  14
#define CAM_PIN_VSYNC 15
#define CAM_PIN_RESET 16
#define CAM_PIN_SIOC  17
#define CAM_PIN_SIOD  18

#define GPIO_DBG_LED  48
#define GPIO_LED      21

// PWM配置
#define LEDC_TIMER          LEDC_TIMER_0
#define LEDC_MODE           LEDC_LOW_SPEED_MODE
#define LEDC_CHANNEL        LEDC_CHANNEL_0
#define LEDC_DUTY_RES       LEDC_TIMER_10_BIT  // 10位分辨率，占空比范围0-1023
#define LEDC_FREQUENCY      40000               // PWM频率40kHz

#define EXAMPLE_ESP_WIFI_SSID      CONFIG_ESP_WIFI_SSID
#define EXAMPLE_ESP_WIFI_PASS      CONFIG_ESP_WIFI_PASSWORD
#define EXAMPLE_ESP_WIFI_CHANNEL   CONFIG_ESP_WIFI_CHANNEL
#define EXAMPLE_MAX_STA_CONN       CONFIG_ESP_MAX_STA_CONN


static void wifi_event_handler(void* arg, esp_event_base_t event_base,
                                    int32_t event_id, void* event_data)
{
    if (event_id == WIFI_EVENT_AP_STACONNECTED) {
        wifi_event_ap_staconnected_t* event = (wifi_event_ap_staconnected_t*) event_data;
        ESP_LOGI(TAG, "station "MACSTR" join, AID=%d",
                 MAC2STR(event->mac), event->aid);
    } else if (event_id == WIFI_EVENT_AP_STADISCONNECTED) {
        wifi_event_ap_stadisconnected_t* event = (wifi_event_ap_stadisconnected_t*) event_data;
        ESP_LOGI(TAG, "station "MACSTR" leave, AID=%d, reason=%d",
                 MAC2STR(event->mac), event->aid, event->reason);
    }
}

void wifi_init_softap(void)
{
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());
    esp_netif_create_default_wifi_ap();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));

    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT,
                                                        ESP_EVENT_ANY_ID,
                                                        &wifi_event_handler,
                                                        NULL,
                                                        NULL));

    wifi_config_t wifi_config = {
        .ap = {
            .ssid = EXAMPLE_ESP_WIFI_SSID,
            .ssid_len = strlen(EXAMPLE_ESP_WIFI_SSID),
            .channel = EXAMPLE_ESP_WIFI_CHANNEL,
            .password = EXAMPLE_ESP_WIFI_PASS,
            .max_connection = EXAMPLE_MAX_STA_CONN,
#ifdef CONFIG_ESP_WIFI_SOFTAP_SAE_SUPPORT
            .authmode = WIFI_AUTH_WPA3_PSK,
            .sae_pwe_h2e = WPA3_SAE_PWE_BOTH,
#else /* CONFIG_ESP_WIFI_SOFTAP_SAE_SUPPORT */
            .authmode = WIFI_AUTH_WPA2_PSK,
#endif
            .pmf_cfg = {
                    .required = true,
            },
        },
    };
    if (strlen(EXAMPLE_ESP_WIFI_PASS) == 0) {
        wifi_config.ap.authmode = WIFI_AUTH_OPEN;
    }

    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_AP));
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_AP, &wifi_config));
    ESP_ERROR_CHECK(esp_wifi_start());

    ESP_LOGI(TAG, "wifi_init_softap finished. SSID:%s password:%s channel:%d",
             EXAMPLE_ESP_WIFI_SSID, EXAMPLE_ESP_WIFI_PASS, EXAMPLE_ESP_WIFI_CHANNEL);
}

static const camera_config_t camera_config = {
    .pin_pwdn = CAM_PIN_PWDN,
    .pin_reset = CAM_PIN_RESET,
    .pin_xclk = CAM_PIN_XCLK,
    .pin_sccb_sda = CAM_PIN_SIOD,
    .pin_sccb_scl = CAM_PIN_SIOC,

    .pin_d7 = CAM_PIN_D7,
    .pin_d6 = CAM_PIN_D6,
    .pin_d5 = CAM_PIN_D5,
    .pin_d4 = CAM_PIN_D4,
    .pin_d3 = CAM_PIN_D3,
    .pin_d2 = CAM_PIN_D2,
    .pin_d1 = CAM_PIN_D1,
    .pin_d0 = CAM_PIN_D0,
    .pin_vsync = CAM_PIN_VSYNC,
    .pin_href = CAM_PIN_HREF,
    .pin_pclk = CAM_PIN_PCLK,

    //XCLK 20MHz or 10MHz for OV2640 double FPS (Experimental)
    .xclk_freq_hz = 20000000,
    .ledc_timer = LEDC_TIMER_1,  // 使用不同的定时器避免冲突
    .ledc_channel = LEDC_CHANNEL_1,

    .pixel_format = PIXFORMAT_RGB565, //YUV422,GRAYSCALE,RGB565,JPEG
    .frame_size = FRAMESIZE_QVGA,    //QQVGA-UXGA, For ESP32, do not use sizes above QVGA when not JPEG. The performance of the ESP32-S series has improved a lot, but JPEG mode always gives better frame rates.
    // 如果数值过小，容易报cam_hal: FB-OVF错误
    // 可以尝试在 menuconfig 中增大 Custom JPEG mode frame size (bytes) 选项的值来增大 jpeg recv buffer 的大小。
    .jpeg_quality = 5, //0-63, for OV series camera sensors, lower number means higher quality
    .fb_count = 2,       //When jpeg mode is used, if fb_count more than one, the driver will work in continuous mode.
    .fb_location = CAMERA_FB_IN_PSRAM,
    .grab_mode = CAMERA_GRAB_WHEN_EMPTY,
};

static esp_err_t init_camera(void)
{
    //initialize the camera
    esp_err_t err = esp_camera_init(&camera_config);
    if (err != ESP_OK)
    {
        ESP_LOGE(TAG, "Camera Init Failed");
        return err;
    }
    // 2. 获取传感器对象
    sensor_t *s = esp_camera_sensor_get();

    // 3. 配置各种参数
    // 配置曝光
    s->set_exposure_ctrl(s, 1);  // 启用自动曝光 (0=关闭, 1=开启)
    s->set_ae_level(s, -3);       // 曝光级别 (-5 ~ +5)
    
    // 配置像素格式
    // s->set_pixformat(s, PIXFORMAT_JPEG); // 或者 PIXFORMAT_RGB565
    
    return ESP_OK;
}

// 初始化PWM控制器
static void init_pwm(void)
{
    // 配置定时器
    ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_MODE,
        .duty_resolution  = LEDC_DUTY_RES,
        .timer_num        = LEDC_TIMER,
        .freq_hz          = LEDC_FREQUENCY,
        .clk_cfg          = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    // 配置通道
    ledc_channel_config_t ledc_channel = {
        .speed_mode     = LEDC_MODE,
        .channel        = LEDC_CHANNEL,
        .timer_sel      = LEDC_TIMER,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = GPIO_LED,
        .duty           = 0,
        .hpoint         = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));

    // 默认亮度设置计算方式
    uint32_t max_duty = (1 << LEDC_DUTY_RES) - 1;  // 1023 for 10-bit
    uint32_t duty = max_duty * led_brightness / 100;
    ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, duty);
    ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
}

// -------------------------------------- 颜色识别代码 -------------------------------------- 
const uint8_t index_mapping[54] = {
    6,3,0,7,4,1,8,5,2,
    47,50,53,46,49,52,45,48,51,
    17,16,15,14,13,12,11,10,9,
    26,25,24,23,22,21,20,19,18,
    38,41,44,37,40,43,36,39,42,
    27,28,29,30,31,32,33,34,35
};

// 添加颜色获取函数
static void capture_and_process_colors(httpd_req_t *req)
{
    if (xSemaphoreTake(camera_mutex, portMAX_DELAY) != pdTRUE) {
        ESP_LOGE(TAG, "Failed to take camera mutex");
        return;
    }
    camera_fb_t *fb = esp_camera_fb_get();
    if (!fb) {
        ESP_LOGE(TAG, "Camera capture failed");
        // s->set_pixformat(s, old_format);
        xSemaphoreGive(camera_mutex);
        return;
    }
    int64_t start_time = esp_timer_get_time();
    // 存储颜色数据
    uint8_t color_data[54][2] = {0}; // [H, S]
    // 处理每个子区域
    for (int i = 0; i < 18; i++) {
        uint8_t dominant_h, dominant_s;
        uint8_t dominant_percentage;
            
        // 直接提取主色调
        extract_dominant_color_from_quad(color_detect_quads[i], fb->buf, fb->width, fb->height, 
                &dominant_h, &dominant_s, &dominant_percentage);
            
        // 输出中间结果
        // ESP_LOGI(TAG, "Region %2d: H=%3d, S=%3d, dominant=%d%%", 
        //     i, dominant_h, dominant_s, dominant_percentage);
            
        // 查询存放的位置
        int index = index_mapping[i];
        color_data[index][0] = dominant_h;
        color_data[index][1] = dominant_s;
    }
    int64_t end_time = esp_timer_get_time();
    ESP_LOGI(TAG, "time cost of extract_dominant_color_from_quad: %.1fms", (end_time - start_time) / 1000.0f);
    // 构建颜色数据字符串
    char color_str[600] = "colors:";
    char temp[20];
    
    for (int i = 0; i < 54; i++) {
        if (i > 0) {
            strcat(color_str, ",");
        }
        snprintf(temp, sizeof(temp), "%d,%d", color_data[i][0], color_data[i][1]);
        strcat(color_str, temp);
    }
    ESP_LOGI(TAG, "hsv: %s", color_str);
    // 发送颜色数据
    httpd_ws_frame_t ws_pkt_resp = {
        .payload = (uint8_t *)color_str,
        .len = strlen(color_str),
        .type = HTTPD_WS_TYPE_TEXT
    };
    httpd_ws_send_frame(req, &ws_pkt_resp);
    
    // ==== 新增部分：保存图像缓冲区 ====
    if (xSemaphoreTake(s_fb_buf_mutex, portMAX_DELAY) == pdTRUE) {
        // 释放旧缓冲区
        if (s_fb_buf) {
            free(s_fb_buf);
            s_fb_buf = NULL;
        }
        
        // 分配新缓冲区并复制数据
        s_fb_buf_len = fb->len;
        s_fb_buf = malloc(s_fb_buf_len);
        if (s_fb_buf) {
            memcpy(s_fb_buf, fb->buf, s_fb_buf_len);
            ESP_LOGI(TAG, "Saved RGB565 buffer, size: %d bytes", s_fb_buf_len);
        } else {
            ESP_LOGE(TAG, "Failed to allocate buffer for RGB565 image");
            s_fb_buf_len = 0;
        }
        xSemaphoreGive(s_fb_buf_mutex);
    }

    // 恢复原始格式
    esp_camera_fb_return(fb);
    
    xSemaphoreGive(camera_mutex);
}

// -------------------------------------- Web界面部分 -------------------------------------- 
#define INDEX_HTML_PATH "/spiffs/index.html"

// 添加视频流处理函数
#define PART_BOUNDARY "123456789000000000000987654321"
static const char* _STREAM_CONTENT_TYPE = "multipart/x-mixed-replace;boundary=" PART_BOUNDARY;
static const char* _STREAM_BOUNDARY = "\r\n--" PART_BOUNDARY "\r\n";
static const char* _STREAM_PART = "Content-Type: image/jpeg\r\nContent-Length: %u\r\n\r\n";

esp_err_t stream_handler(httpd_req_t *req){
    camera_fb_t * fb = NULL;
    esp_err_t res = ESP_OK;
    size_t _jpg_buf_len;
    uint8_t * _jpg_buf;
    char * part_buf[64];
    // 新增统计变量（静态变量保持状态）
    static int64_t last_output = 0;        // 上次输出时间
    static size_t max_frame_size = 0;      // 5秒内最大的图像帧长度
    static int frame_count = 0;            // 5秒周期内帧计数
    if (!last_output) {
        last_output = esp_timer_get_time();
    }

    res = httpd_resp_set_type(req, _STREAM_CONTENT_TYPE);
    if(res != ESP_OK){
        return res;
    }

    while(true){
        // 获取互斥锁
        if (xSemaphoreTake(camera_mutex, portMAX_DELAY) != pdTRUE) {
            ESP_LOGE(TAG, "Failed to take camera mutex");
            res = ESP_FAIL;
            break;
        }
        fb = esp_camera_fb_get();
        if (!fb) {
            ESP_LOGE(TAG, "Camera capture failed");
            xSemaphoreGive(camera_mutex);  // 释放锁
            res = ESP_FAIL;
            break;
        }
        if(fb->format != PIXFORMAT_JPEG){
            bool jpeg_converted = frame2jpg(fb, 80, &_jpg_buf, &_jpg_buf_len);
            if(!jpeg_converted){
                ESP_LOGE(TAG, "JPEG compression failed");
                esp_camera_fb_return(fb);
                res = ESP_FAIL;
            }
        } else {
            _jpg_buf_len = fb->len;
            _jpg_buf = fb->buf;
        }
        if(res == ESP_OK){
            res = httpd_resp_send_chunk(req, _STREAM_BOUNDARY, strlen(_STREAM_BOUNDARY));
        }
        if(res == ESP_OK){
            size_t hlen = snprintf((char *)part_buf, 64, _STREAM_PART, _jpg_buf_len);

            res = httpd_resp_send_chunk(req, (const char *)part_buf, hlen);
        }
        if(res == ESP_OK){
            res = httpd_resp_send_chunk(req, (const char *)_jpg_buf, _jpg_buf_len);
        }
        if(fb->format != PIXFORMAT_JPEG){
            free(_jpg_buf);
        }
        esp_camera_fb_return(fb);
        xSemaphoreGive(camera_mutex);  // 释放锁
        if(res != ESP_OK){
            break;
        }
        // 更新周期统计
        frame_count++;
        if(_jpg_buf_len > max_frame_size ){
            max_frame_size = _jpg_buf_len;
        }
        // 计算当前帧时间
        int64_t now = esp_timer_get_time();
        
        // 检查是否达到5秒输出周期
        if (now - last_output >= 5000000) {  // 5秒 = 5,000,000微秒
            float period_seconds = (now - last_output) / 1000000.0f;
            float avg_fps = frame_count / period_seconds;
            
            ESP_LOGI(TAG, "5s Stats: Avg FPS=%.1f, max_frame_size=%zuKB", avg_fps, max_frame_size / 1024);
            
            // 重置统计状态
            frame_count = 0;
            max_frame_size = 0;
            last_output = now;
        }
        vTaskDelay(1);// 短暂释放CPU，可调度其他任务
    }
    return res;
}

// 只挂载SPIFFS不加载文件
static void mount_spiffs(void)
{
    esp_vfs_spiffs_conf_t conf = {
        .base_path = "/spiffs",
        .partition_label = NULL,
        .max_files = 5,
        .format_if_mount_failed = true};

    ESP_ERROR_CHECK(esp_vfs_spiffs_register(&conf));
}

// 修改请求处理函数，直接从文件系统读取并发送
esp_err_t get_req_handler(httpd_req_t *req)
{
    // 打开文件
    FILE *fp = fopen(INDEX_HTML_PATH, "r");
    if (!fp) {
        ESP_LOGE(TAG, "Failed to open index.html");
        const char *resp = "404 Not Found";
        httpd_resp_send(req, resp, HTTPD_RESP_USE_STRLEN);
        return ESP_FAIL;
    }

    // 获取文件大小
    struct stat st;
    if (stat(INDEX_HTML_PATH, &st)) {
        fclose(fp);
        ESP_LOGE(TAG, "Failed to stat index.html");
        const char *resp = "500 Internal Server Error";
        httpd_resp_send(req, resp, HTTPD_RESP_USE_STRLEN);
        return ESP_FAIL;
    }

    // 设置HTTP响应头
    httpd_resp_set_type(req, "text/html");
    
    // 分块发送文件内容
    char buffer[256];
    size_t read_bytes;
    while ((read_bytes = fread(buffer, 1, sizeof(buffer), fp)) > 0) {
        if (httpd_resp_send_chunk(req, buffer, read_bytes) != ESP_OK) {
            fclose(fp);
            ESP_LOGE(TAG, "File sending failed");
            return ESP_FAIL;
        }
    }
    
    // 结束分块传输
    httpd_resp_send_chunk(req, NULL, 0);
    fclose(fp);
    return ESP_OK;
}

static esp_err_t handle_ws_req(httpd_req_t *req)
{
    if (req->method == HTTP_GET)
    {
        ESP_LOGI(TAG, "Handshake done, the new connection was opened");
        return ESP_OK;
    }

    httpd_ws_frame_t ws_pkt;
    uint8_t *buf = NULL;
    memset(&ws_pkt, 0, sizeof(httpd_ws_frame_t));
    ws_pkt.type = HTTPD_WS_TYPE_TEXT;
    esp_err_t ret = httpd_ws_recv_frame(req, &ws_pkt, 0);
    if (ret != ESP_OK)
    {
        ESP_LOGE(TAG, "httpd_ws_recv_frame failed to get frame len with %d", ret);
        return ret;
    }

    if (ws_pkt.len)
    {
        buf = calloc(1, ws_pkt.len + 1);
        if (buf == NULL)
        {
            ESP_LOGE(TAG, "Failed to calloc memory for buf");
            return ESP_ERR_NO_MEM;
        }
        ws_pkt.payload = buf;
        ret = httpd_ws_recv_frame(req, &ws_pkt, ws_pkt.len);
        if (ret != ESP_OK)
        {
            ESP_LOGE(TAG, "httpd_ws_recv_frame failed with %d", ret);
            free(buf);
            return ret;
        }
        ESP_LOGI(TAG, "Got packet with message: %s", ws_pkt.payload);
    }

    ESP_LOGI(TAG, "frame len is %d", ws_pkt.len);

    if (ws_pkt.type == HTTPD_WS_TYPE_TEXT)
    {
        if (strcmp((char *)ws_pkt.payload, "get_brightness") == 0) {
            char buff[32];
            sprintf(buff, "brightness:%d", led_brightness);
            httpd_ws_frame_t ws_pkt_resp = {
                .payload = (uint8_t *)buff,
                .len = strlen(buff),
                .type = HTTPD_WS_TYPE_TEXT
            };
            httpd_ws_send_frame(req, &ws_pkt_resp);
            free(buf);
            return ESP_OK;
        }
        // 处理亮度控制消息
        else if (strncmp((char *)ws_pkt.payload, "brightness:", 11) == 0) {
            led_brightness = atoi((char *)ws_pkt.payload + 11);
            if (led_brightness < 0) led_brightness = 0;
            if (led_brightness > 100) led_brightness = 100;
            // 设置PWM占空比
            uint32_t max_duty = (1 << LEDC_DUTY_RES) - 1;  // 1023 for 10-bit
            uint32_t duty = max_duty * led_brightness / 100;
            ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, duty);
            ledc_update_duty(LEDC_MODE, LEDC_CHANNEL);
            char buff[32];
            sprintf(buff, "brightness:%d", led_brightness);
            httpd_ws_frame_t ws_pkt_resp = {
                .payload = (uint8_t *)buff,
                .len = strlen(buff),
                .type = HTTPD_WS_TYPE_TEXT
            };
            httpd_ws_send_frame(req, &ws_pkt_resp);
            free(buf);
            return ESP_OK;
        }
        // 处理拍照命令
        else if (strcmp((char *)ws_pkt.payload, "toggle") == 0) {
            ESP_LOGI(TAG, "toggle button pressed");
            free(buf);
            return ESP_OK;
        } 
        // 处理坐标更新消息
        else if (strncmp((char *)ws_pkt.payload, "update_pos:", 11) == 0) {
            char *pos_str = (char *)ws_pkt.payload + 11;
            ESP_LOGI(TAG, "Received positions: %s", pos_str);
            
            // 解析坐标字符串 (格式: (x1,y1),(x2,y2),...,(x6,y6))
            char *token = strtok(pos_str, "(),");
            for (int i = 0; i < 6 && token != NULL; i++) {
                // 读取x坐标
                color_detect_points[i].x = atoi(token);
                token = strtok(NULL, "(),");
                // 读取y坐标
                if (token != NULL) {
                    color_detect_points[i].y = atoi(token);
                    token = strtok(NULL, "(),");
                }
                // ESP_LOGI(TAG, "Point %d: (%d, %d)", i, 
                //          color_detect_points[i].x, color_detect_points[i].y);
            }
            
            // 更新颜色检测四边形
            get_2x3x3_quads(color_detect_points, color_detect_quads);

            free(buf);
            return ESP_OK;
        }
        // 处理获取坐标请求
        else if (strcmp((char *)ws_pkt.payload, "get_pos") == 0) {
            // 格式化坐标字符串 (格式: (x1,y1),(x2,y2),...,(x6,y6))
            char pos_str[100];
            snprintf(pos_str, sizeof(pos_str), 
                     "positions:(%hd,%hd),(%hd,%hd),(%hd,%hd),(%hd,%hd),(%hd,%hd),(%hd,%hd)",
                     color_detect_points[0].x, color_detect_points[0].y,
                     color_detect_points[1].x, color_detect_points[1].y,
                     color_detect_points[2].x, color_detect_points[2].y,
                     color_detect_points[3].x, color_detect_points[3].y,
                     color_detect_points[4].x, color_detect_points[4].y,
                     color_detect_points[5].x, color_detect_points[5].y);
            
            httpd_ws_frame_t ws_pkt_resp = {
                .payload = (uint8_t *)pos_str,
                .len = strlen(pos_str),
                .type = HTTPD_WS_TYPE_TEXT
            };
            httpd_ws_send_frame(req, &ws_pkt_resp);
            free(buf);
            return ESP_OK;
        }
        // 处理获取颜色请求
        else if (strcmp((char *)ws_pkt.payload, "get_colors") == 0) {
            ESP_LOGI(TAG, "Capturing and processing colors");
            capture_and_process_colors(req);
            free(buf);
            return ESP_OK;
        }
    }
    free(buf);
    return ESP_OK;
}
// 添加新的HTTP处理函数用于下载图像缓冲区
static esp_err_t download_fb_buf_handler(httpd_req_t *req)
{
    if (xSemaphoreTake(s_fb_buf_mutex, portMAX_DELAY) != pdTRUE) {
        ESP_LOGE(TAG, "Failed to take fb_buf mutex");
        return ESP_FAIL;
    }
    
    if (!s_fb_buf || s_fb_buf_len == 0) {
        xSemaphoreGive(s_fb_buf_mutex);
        httpd_resp_send_404(req);
        return ESP_FAIL;
    }
    
    // 设置HTTP响应头
    httpd_resp_set_type(req, "application/octet-stream");
    httpd_resp_set_hdr(req, "Content-Disposition", "attachment; filename=fb_buf.bin");
    
    // 发送图像数据
    esp_err_t ret = httpd_resp_send(req, (const char *)s_fb_buf, s_fb_buf_len);
    
    xSemaphoreGive(s_fb_buf_mutex);
    return ret;
}
void setup_servers(void) {
    // 主服务器配置 (处理WebSocket等)
    httpd_config_t config = HTTPD_DEFAULT_CONFIG();
    config.server_port = 80;
    config.max_uri_handlers = 16;  // 增加最大URI处理程序数量
    
    if (httpd_start(&camera_httpd, &config) == ESP_OK) {
        ESP_LOGI(TAG, "Main HTTP server started on port %d", config.server_port);
        
        // 注册WebSocket和网页请求
        httpd_uri_t uri_get = {
            .uri = "/",
            .method = HTTP_GET,
            .handler = get_req_handler,
            .user_ctx = NULL
        };
        httpd_register_uri_handler(camera_httpd, &uri_get);
        
        httpd_uri_t ws = {
            .uri = "/ws",
            .method = HTTP_GET,
            .handler = handle_ws_req,
            .user_ctx = NULL,
            .is_websocket = true
        };
        httpd_register_uri_handler(camera_httpd, &ws);

        // 注册下载图像缓冲区的URI
        httpd_uri_t download_uri = {
            .uri = "/fb_buf.bin",
            .method = HTTP_GET,
            .handler = download_fb_buf_handler,
            .user_ctx = NULL
        };
        httpd_register_uri_handler(camera_httpd, &download_uri);
    } else {
        ESP_LOGE(TAG, "Failed to start main HTTP server on port %d", config.server_port);
    }
    
    // 流服务器配置 (专门处理视频流)
    httpd_config_t stream_config = HTTPD_DEFAULT_CONFIG();
    stream_config.server_port = 81;  // 使用不同端口
    stream_config.ctrl_port += 1;    // 增加控制端口
    
    if (httpd_start(&stream_httpd, &stream_config) == ESP_OK) {
        ESP_LOGI(TAG, "Stream HTTP server started on port %d", stream_config.server_port);
        
        httpd_uri_t stream_uri = {
            .uri = "/stream",
            .method = HTTP_GET,
            .handler = stream_handler,
            .user_ctx = NULL
        };
        httpd_register_uri_handler(stream_httpd, &stream_uri);
    } else {
        ESP_LOGE(TAG, "Failed to start stream HTTP server on port %d", stream_config.server_port);
    }
}

// -------------------------------------- main -------------------------------------- 
void app_main(void)
{
    //Initialize NVS
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    ESP_LOGI(TAG, "INIT_WIFI");
    wifi_init_softap();
    ESP_LOGI(TAG, "INIT_IO");
    init_pwm();
    init_rs485();
    ESP_LOGI(TAG, "INIT_CAMERA");
    camera_mutex = xSemaphoreCreateMutex();
    if (camera_mutex == NULL) {
        ESP_LOGE(TAG, "Failed to create camera mutex");
        return;
    }
    s_fb_buf_mutex = xSemaphoreCreateMutex();// 创建图像缓冲区互斥锁
    if (s_fb_buf_mutex == NULL) {
        ESP_LOGE(TAG, "Failed to create fb_buf mutex");
        return;
    }
    if(ESP_OK != init_camera()) {
        return;
    }
    ESP_LOGI(TAG, "INIT_WEB");
    mount_spiffs();
    setup_servers();
    ESP_LOGI(TAG, "LOAD_PARM");
    // 从FLASH中加载color_detect_points，并且重新计算color_detect_quads
    // TODO 从FLASH加载参数
    get_2x3x3_quads(color_detect_points, color_detect_quads);
    while (1)
    {
        cmd_zero();
        vTaskDelay(3000 / portTICK_PERIOD_MS);
        // 使能指令数据帧: ffff1104010100020100030100040100c9
        // 接收到响应帧: ffff0500e2
        uint8_t ids[] = {1, 2, 3, 4};
        if (cmd_enable(ids, 4, 0) == 0)
        {
            ESP_LOGI(TAG, "禁用成功");
        }
        else
        {
            ESP_LOGE(TAG, "禁用失败");
        }
        vTaskDelay(3000 / portTICK_PERIOD_MS);
    }
}